#include <iostream>
#include <cmath>
#include <stdlib.h>
#include <time.h>
#include <stack>
#include "main.h"


#include "WorldStatusUpdate.pb.h"
#include "Commands.pb.h"
#include "ProtobufUtil.hh"

using namespace strategy_protocol;
using namespace protoutil;
using namespace std;

Position TheStrategy::nextstep(Position p,Position q,bool approx,bool considering_people)
{
	cerr << "Nextstep: " << p.x()-translation_x << p.y()-translation_y << " --> " << q.x()-translation_x << q.y()-translation_y<< (approx?" APPROX ":" EXACT ");
	vector<vector<int> > d;
	vector<vector<int> > pred_x;
	vector<vector<int> > pred_y;
	d.resize(M);
	pred_x.resize(M);
	pred_y.resize(M);
	for(int i=0;i<M;i++)
		{
			d[i].resize(N,M*N);
			pred_x[i].resize(N);
			pred_y[i].resize(N);
		}
	d[p.x()-translation_x][p.y()-translation_y]=0;
	pred_x[p.x()-translation_x][p.y()-translation_y]=p.x()-translation_x;
	pred_y[p.x()-translation_x][p.y()-translation_y]=p.y()-translation_y;
	bool done=false;
	while(!done)
	{
		done=true;
		for(int i=0;i<M;i++)
			for(int j=0;j<N;j++)
			{
				bool valid_pos = considering_people ? mymap[i][j]==' ' || mymap[i][j]=='M' :  !forbidden(mymap[i][j]);
				if(valid_pos || (i==q.x()-translation_x && j==q.y()-translation_y))
					for(int ii=-1;ii<=1;ii++)
						for(int jj=-1;jj<=1;jj++)
						{
							if(ii!=jj && i+ii>=0 && i+ii<M && j+jj>=0 && j+jj<N &&
								d[i][j]>d[i+ii][j+jj]+1)
								{
									d[i][j]=d[i+ii][j+jj]+1; pred_x[i][j]=i+ii; pred_y[i][j]=j+jj; done=false;
									// cerr << i << j << " " << i+ii << j+jj << " " << d[i][j] << endl;
								}
						}
			}
		if(d[q.x()-translation_x][q.y()-translation_y]!=M*N) done=true;
	}

	cerr << " real_distance: "<< d[q.x()-translation_x][q.y()-translation_y];

	int x=q.x()-translation_x,y=q.y()-translation_y;
	if(d[x][y]==M*N)
	{
		if(approx)
		{
			int k=1,i,j;
			bool jo=false;
			while(!jo)
			{
				for(i=-k;i<=k && !jo;i++)
					for(j=-k;j<=k && !jo;j++)
						if(x+i>=0 && x+i<M && y+j>=0 && y+j<N && d[x+i][y+j]!=M*N) jo=true;
				k++;
			}
			x=x+i-1;
			y=y+j-1;
			cerr << " approx to " << x << y;
		}
		else
			return p;
	}

	while(pred_x[x][y]!=p.x()-translation_x || pred_y[x][y]!=p.y()-translation_y)
	{
		// cerr << x << y << " "<<d[x][y]<<endl;
		int xx=pred_x[x][y];
		int yy=pred_y[x][y];
		x=xx; y=yy;
	}
	cerr << " next pos: " << x << y << " DONE" << endl;
	Position next;
	next.set_x(x+translation_x);
	next.set_y(y+translation_y);
	return next;
}

/// feltetelezzuk, hogy van út
vector<Position> TheStrategy::route(Position p,Position q,vector<Position> forbidden_pos)
{
	vector<vector<int> > d;
	vector<vector<int> > pred_x;
	vector<vector<int> > pred_y;
	d.resize(M);
	pred_x.resize(M);
	pred_y.resize(M);
	for(int i=0;i<M;i++)
		{
			d[i].resize(N,M*N);
			pred_x[i].resize(N);
			pred_y[i].resize(N);
		}
	d[p.x()-translation_x][p.y()-translation_y]=0;
	pred_x[p.x()-translation_x][p.y()-translation_y]=p.x()-translation_x;
	pred_y[p.x()-translation_x][p.y()-translation_y]=p.y()-translation_y;
	bool done=false;
	while(!done)
	{
		done=true;
		for(int i=0;i<M;i++)
			for(int j=0;j<N;j++)
			{
				int cost = 1;
				bool valid_pos = !forbidden(mymap[i][j]);
				for(unsigned int k=1;k<forbidden_pos.size() && valid_pos;k++)
					if(forbidden_pos[k].x()-translation_x==i && forbidden_pos[k].y()-translation_y==j) cost = sqrt(M*N);
				if(valid_pos || (i==q.x()-translation_x && j==q.y()-translation_y))
					for(int ii=-1;ii<=1;ii++)
						for(int jj=-1;jj<=1;jj++)
						{
							if(ii!=jj &&  i+ii>=0 && i+ii<M && j+jj>=0 && j+jj<N &&
								d[i][j]>d[i+ii][j+jj]+cost)
								{
									d[i][j]=d[i+ii][j+jj]+cost; pred_x[i][j]=i+ii; pred_y[i][j]=j+jj; done=false;
									// cerr << i << j << " " << i+ii << j+jj << " " << d[i][j] << endl;
								}
						}
			}
//		if(d[q.x()-translation_x][q.y()-translation_y]!=M*N) done=true;
	}

	stack<Position> route;
	int x=q.x()-translation_x,y=q.y()-translation_y;
	while(pred_x[x][y]!=p.x()-translation_x || pred_y[x][y]!=p.y()-translation_y)
	{
		int xx=pred_x[x][y];
		int yy=pred_y[x][y];
		x=xx; y=yy;
		Position tmp;
//		cerr << x << y << " "<<d[x][y]<<endl;
		tmp.set_x(x+translation_x);
		tmp.set_y(y+translation_y);
		route.push(tmp);
	}
	cerr << "Routing DONE" << endl;
	vector<Position> retroute;
	while(!route.empty())
	{
		retroute.push_back(route.top());
		route.pop();
	}
	return retroute;
}

int TheStrategy::real_distance(Position p,Position q,bool considering_people)
{
    vector<Position> empty_r;
    return real_distance(p,q,considering_people,empty_r);
}

int TheStrategy::real_distance(Position p,Position q,bool considering_people,vector<Position> forbidden_pos)
{
    cerr << "Computing real distance: " << p.x()-translation_x << p.y()-translation_y << " --> " << q.x()-translation_x << q.y()-translation_y<< ": ";
	vector<vector<int> > d;
	vector<vector<int> > pred_x;
	vector<vector<int> > pred_y;
	d.resize(M);
	pred_x.resize(M);
	pred_y.resize(M);
	for(int i=0;i<M;i++)
		{
			d[i].resize(N,M*N);
			pred_x[i].resize(N);
			pred_y[i].resize(N);
		}
	d[p.x()-translation_x][p.y()-translation_y]=0;
	pred_x[p.x()-translation_x][p.y()-translation_y]=p.x()-translation_x;
	pred_y[p.x()-translation_x][p.y()-translation_y]=p.y()-translation_y;
	bool done=false;
	while(!done)
	{
		done=true;
		for(int i=0;i<M;i++)
			for(int j=0;j<N;j++)
			{
				int cost = 1;
				bool valid_pos = !forbidden(mymap[i][j]);
				for(unsigned int k=1;k<forbidden_pos.size() && valid_pos;k++)
					if(forbidden_pos[k].x()-translation_x==i && forbidden_pos[k].y()-translation_y==j) cost = sqrt(M*N);
				if(valid_pos || (i==q.x()-translation_x && j==q.y()-translation_y))
					for(int ii=-1;ii<=1;ii++)
						for(int jj=-1;jj<=1;jj++)
						{
							if(ii!=jj &&  i+ii>=0 && i+ii<M && j+jj>=0 && j+jj<N &&
								d[i][j]>d[i+ii][j+jj]+cost)
								{
									d[i][j]=d[i+ii][j+jj]+cost; pred_x[i][j]=i+ii; pred_y[i][j]=j+jj; done=false;
									// cerr << i << j << " " << i+ii << j+jj << " " << d[i][j] << endl;
								}
						}
			}
		if(d[q.x()-translation_x][q.y()-translation_y]!=M*N) done=true;
	}

    cerr<<d[q.x()-translation_x][q.y()-translation_y]<<" DONE"<<endl;
	return d[q.x()-translation_x][q.y()-translation_y];
}


void TheStrategy::computing_all_reachable_positions_from_base()
{
	reachable_pos.clear();
	reachable_pos.resize(M);
	for(int i=0;i<M;i++)
		reachable_pos[i].resize(N,false);
	reachable_pos[base_pos.x()-translation_x][base_pos.y()-translation_y]=true;
	bool done=false;
	while(!done)
	{
		done=true;
		for(int i=0;i<M;i++)
			for(int j=0;j<N;j++)
				if(!forbidden(mymap[i][j]))
					for(int ii=-1;ii<=1;ii++)
						for(int jj=-1;jj<=1;jj++)
							if(ii!=jj && i+ii>=0 && i+ii<M && j+jj>=0 && j+jj<N && !reachable_pos[i][j] &&
                                reachable_pos[i+ii][j+jj] && discovered[i+ii][j+jj])
								{
									reachable_pos[i][j]=true; done=false;
//									cerr <<i<<","<<j<< " reachable" << endl;
								}
	}
}
